
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_local_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/buffer.h>

namespace yyc_planner{
    class MyLocalPlanner: public nav_core::BaseLocalPlanner{
        public:
        MyLocalPlanner();
        
        bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
        bool isGoalReached();
        bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan);
        void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
        public:
        tf2_ros::Buffer* tf_; ///< @brief Used for transforming point clouds
        costmap_2d::Costmap2DROS* costmap_ros_;
        geometry_msgs::PoseStamped current_pose_;
        std::vector<geometry_msgs::PoseStamped> global_plan_;
    };
};